package com.hitqz.robot.driver.zmq.protocol.handler;

import cn.hutool.core.collection.CollUtil;
import com.example.tutorial.AddressBookProtos;
import com.hitqz.robot.driver.dto.PoseDto;
import com.hitqz.robot.driver.util.WheelCacheUtil;
import com.hitqz.robot.driver.util.WheelToolkit;
import lombok.extern.slf4j.Slf4j;
import navconnector.NavconnectorBase;
import org.springframework.stereotype.Component;

import java.util.ArrayList;
import java.util.List;

/**
 * 激光点云信息处理类
 *
 * @author wengyx
 * @date 2021-09-14 10:08
 **/
@Slf4j
@Component
public class PointCloudHandler implements HandlerZMQService {



    @Override
    public void handlerMsg(String robotSn, byte[] data) throws Exception {
        AddressBookProtos.PointCloud pointCloud = AddressBookProtos.PointCloud.parseFrom(data);
        String type = pointCloud.getDescriptorForType().getName();
        int ackId = pointCloud.getHeader().getAck();
        int objId = pointCloud.getHeader().getObj();
        // 3D激光点云不需要坐标转换 主动上报
        if (objId == AddressBookProtos.PointCloud.OBJ_ID.NUL_SCAN_FRAME_NTF_VALUE) {
            List<NavconnectorBase.Point> scanFrame = pointCloud.getPointsList();
            List<PoseDto> list = new ArrayList<>();
            for (NavconnectorBase.Point point : CollUtil.emptyIfNull(scanFrame)) {
                PoseDto pointDto = new PoseDto();
                pointDto.setX((float) point.getX());
                pointDto.setY((float) point.getY());
                pointDto.setZ((float) point.getZ());
                list.add(pointDto);
            }
            //现在和2D的放同一个key，以前是分开放
            WheelCacheUtil.put(WheelCacheUtil.KEY_LASER_SCAN,list);
        }
        //释放锁
        WheelToolkit.release(ackId,type, objId, true);
    }

}
